import Tkinter
from Tkinter import *
import protocol
from threading import Thread
import pygame
import autopilot

WIDTH = 2
LEFT_LABEL_COL = 0
PADDING =10
DATA_ROW=6
LABEL_SPAN = 3
DATA_SPAN = 2
INFO_SPAN = LABEL_SPAN + DATA_SPAN
GRID_WIDTH = 13
PAD_START = 0
NUM_MEASUREMENTS = 5

manual_control = False
m_r = 0
m_l = 0
m_v = 0

class Gui:
    # sensor measurements: Down, left, right, front, back, battery, regulator
    def set_label_values(self,sensor_measurements):
        for i in range(len(sensor_measurements)):
            self.measurements[i].config(text='%.2f'%sensor_measurements[i])

    measurements=[]
    def __init__(self, master):
        frame = Entry(master)
        frame.grid(row = 10,column = 7)
        self.initializeLabels(frame)
        self.initializeButtons(frame)
        # create a button in the frame(?) with the given text and callback fn

    def changeLabelText(self,label, new_text):
        label.config(text=new_text,justify = RIGHT)

    def print_motor_box(self):
        print("Motor power: " + self.motor_power.get())

    def set_motors(self,R,L,V):
        if not manual_control:
            global m_r, m_l, m_v
            power = self.motor_power.get()
            print power
            power = 50#int(power)
            if power > 120: power = 120
            if power < 0:   power = 0
            m_r = int(R*power)
            m_l = int(L*power)
            m_v = int(V*power)

    def initializeButtons(self,frame):
        self.forward_button = Button(frame, text="Forward", fg = "black",
                                 command = lambda: self.set_motors(1,1,0))
        self.forward_button.grid(row = 0, column = PAD_START + 2, columnspan = 2, sticky=E)

        self.left_forward_button = Button(frame, text="F-Left ", fg = "black",
                                 command = lambda: self.set_motors(0,1,0))
        self.left_forward_button.grid(row = 0, column = PAD_START, columnspan = 2, sticky=W)

        self.right_forward_button = Button(frame, text="F-Right", fg = "black",
                                 command = lambda: self.set_motors(1,0,0))
        self.right_forward_button.grid(row = 0, column = PAD_START +4, columnspan = 2, sticky=E)


        self.left_turn_button = Button(frame, text="<---", fg = "black",
                                 command = lambda: self.set_motors(-1,1,0))
        self.left_turn_button.grid(row = 1, column = PAD_START, columnspan = 2, sticky=W)

        self.right_turn_button = Button(frame, text="--->", fg = "black",
                                 command = lambda: self.set_motors(1,-1,0))
        self.right_turn_button.grid(row = 1, column = PAD_START + 4, columnspan = 2, sticky=E)


        self.stop_button = Button(frame, text="STOP", fg = "RED",
                                 command = lambda: self.set_motors(0,0,0))
        self.stop_button.grid(row = 1, column = PAD_START + 2, columnspan = 2)


        self.reverse_button = Button(frame, text="Reverse", fg = "black",
                                 command = lambda: self.set_motors(-1,-1,0))
        self.reverse_button.grid(row = 2, column = PAD_START + 2, columnspan = 2, sticky=E)

        self.left_reverse_button = Button(frame, text="R-Left ", fg = "black",
                                 command = lambda: self.set_motors(0,-1,0))
        self.left_reverse_button.grid(row = 2, column = PAD_START, columnspan = 2, sticky=W)

        self.right_reverse_button = Button(frame, text="R-Right", fg = "black",
                                 command = lambda: self.set_motors(-1,0,0))
        self.right_reverse_button.grid(row = 2, column = PAD_START + 4, columnspan = 2, sticky=E)

        self.up_button = Button(frame, text="  Up  ", fg = "black",
                                 command = lambda: self.set_motors(0,0,1))
        self.up_button.grid(row = 3, column = PAD_START, columnspan = 3, sticky=W)

        self.down_button = Button(frame, text="Down", fg = "black",
                                 command = lambda: self.set_motors(0,0,-1))
        self.down_button.grid(row = 3, column = PAD_START + 4, columnspan = 3, sticky=E)

        self.GoTo_button = Button(frame, text="GoTo Height", fg = "black",
                                 command = self.print_motor_box)
        self.GoTo_button.grid(row = 4, column = PAD_START, columnspan = 3, sticky=W)

        self.manual_button = Button(frame, text="Switch to PS3 Control", fg = "black",
                                 command = self.manual_button_cb)
        
        self.manual_button.grid(row = 5, column = PAD_START, columnspan = 7, stick = W)

    def manual_button_cb(self):
        global manual_control
        if manual_control:
            self.manual_button.config(text = 'Switch to GUI Control')
        else:
            self.manual_button.config(text = 'Switch to PS3 Control')
        manual_control = not manual_control

    def initializeLabels(self,frame):
        self.motor = Label(frame, text = "Motor Power: ", justify = LEFT)
        self.motor.grid(row = DATA_ROW, column = 0, columnspan=3, sticky = W)
        self.motor_power = Entry(frame,width = 13)
        self.motor_power.grid(row = DATA_ROW, column = 3, columnspan = 5,sticky = W)
        self.motor_power = Entry(frame,width = 10)
        self.motor_power.grid(row = DATA_ROW-2, column = 3, columnspan = 5,sticky = W)
        self.meters = Label(frame, text = "m", justify = LEFT)
        self.meters.grid(row = DATA_ROW-2, column = 5, columnspan=1, sticky = E)
        #sensors = ["DOWN: ","LEFT: ","RIGHT: ", "FRONT: " ,"BACK: ", "BATTERY: ", "REGULATOR: "]
        sensors = protocol.SensorInfo.channels
        for i in range(0, len(sensors)):
            current_string = sensors[i]
            current = Label(frame, text =current_string, justify = LEFT).grid(row = DATA_ROW+1+i,
                       column = LEFT_LABEL_COL,
                       columnspan = LABEL_SPAN,
                       sticky = W)
            current_data = Label(frame, text = "N/A")
            current_data.grid(row = DATA_ROW+1+i,
                       column = LEFT_LABEL_COL + LABEL_SPAN,
                       columnspan = DATA_SPAN,sticky = E)
            self.measurements.append(current_data)
if __name__ == "__main__":
    pygame.init()
    js = pygame.joystick.Joystick(0)
    js.init()
    n_axes = js.get_numaxes()
    n_buttons = js.get_numbuttons()

    clock = pygame.time.Clock()

    while True:
        #for event in pygame.event.get():
        pygame.event.pump()
        axes = [js.get_axis(i) for i in (0,1,2,3)]
        vals = get_motor_vals(axes)
        print vals
        set_motor(vals)
        get_sensor_vals()


def controller(gui, dl):
    global manual_control, m_l, m_r, m_v
    
    sensors = protocol.SensorInfo(dl)
    mc = protocol.MotorControl(dl)
    while True:
        ir = sensors.get_sensors()
        gui.set_label_values(ir.values())
        if manual_control:
            mc.set_motors(0,0,0)
        else:
            mc.set_motors(m_l, m_r, m_v)

dl = protocol.Datalink('/dev/ttyUSB0')
root = Tk()
gui = Gui(root)
t = Thread(target = controller, args=(gui,dl))
t.daemon = True
t.start()
root.mainloop()
